/////////////////////////////////////////////////////////////////////////////
// Name:        SCDisplayFrame.cpp
// Purpose:     
// Author:      Erwan Le Martelot
// Modified by: 
// Created:     18/04/2007 12:58:36
// RCS-ID:      
// Copyright:   
// Licence:     
/////////////////////////////////////////////////////////////////////////////

#if defined(__GNUG__) && !defined(NO_GCC_PRAGMA)
#pragma implementation "SCDisplayFrame.hpp"
#endif

// For compilers that support precompilation, includes "wx/wx.h".
#include "wx/wxprec.h"

#ifdef __BORLANDC__
#pragma hdrstop
#endif

#ifndef WX_PRECOMP
#include "wx/wx.h"
#endif

////@begin includes
////@end includes

#include "SCDisplayFrame.hpp"

// ----- My Includes ----- //

// OpenGL
#ifdef __WXMAC__
#  ifdef __DARWIN__
#    include <OpenGL/glu.h>
#  else
#    include <glu.h>
#  endif
#else
#  include <GL/glu.h>
#endif

// Stringstream
#include <sstream>

// Exceptions
#include "AssertionException.hpp"
#include "SizeException.hpp"
#include "ConstraintSatisfactionException.hpp"

// TkMacros
#include "TKMacros.h"

// Drawing macros
#define CONE_HEIGHT 0.1
#define SYS_SIZE	0.2f
#define SYS_SPACE	0.2f
#define NB_ARROWS	3
#define BG_COLOUR		1.f, 0.95f, 0.75f
#define INPUT_COLOUR	0.f, 1.f, 0.f
#define HIDDEN_COLOUR	0.f, 0.5f, 1.f
#define OUTPUT_COLOUR	1.f, 0.f, 0.f
#define ARC_COLOUR		1.f, 0.625f, 0.f

// ----------------------- //

// ---- My Namespaces ---- //
using namespace std;
using namespace SC;
using namespace Exceptions;
using namespace Types;
// ----------------------- //

////@begin XPM images
////@end XPM images

// Storage for quadratic objects
static GLUquadricObj * quadratic;

// Display lists
static GLuint sphere_list, cone_list;

// 3D objects subdivisions
#define SUBDIVISIONS 50

/*!
 * SCDisplayFrame type definition
 */

IMPLEMENT_CLASS( SCDisplayFrame, wxGLCanvas )

/*!
 * SCDisplayFrame event table definition
 */

BEGIN_EVENT_TABLE( SCDisplayFrame, wxGLCanvas )

////@begin SCDisplayFrame event table entries
	EVT_SIZE( SCDisplayFrame::OnSize )
	EVT_PAINT( SCDisplayFrame::OnPaint )

////@end SCDisplayFrame event table entries

END_EVENT_TABLE()

/*!
 * SCDisplayFrame constructors
 */

SCDisplayFrame::SCDisplayFrame( wxWindow* parent, wxWindowID id, const wxPoint& pos, const wxSize& size, long style, const wxString& name, int* attribs ):
  wxGLCanvas(parent, id, pos, size, style | wxFULL_REPAINT_ON_RESIZE, name)
{
	Init();
	Create(parent, id, pos, size, style, name, attribs);
	InitGL();
}

/*!
 * SCDisplayFrame creator
 */

bool SCDisplayFrame::Create( wxWindow* parent, wxWindowID id, const wxPoint& pos, const wxSize& size, long style, const wxString& name, int* attribs )
{
////@begin SCDisplayFrame creation
	CreateControls();
////@end SCDisplayFrame creation
	return true;
}

/*!
 * Control creation for SCDisplayFrame
 */

void SCDisplayFrame::CreateControls()
{    
////@begin SCDisplayFrame content construction
////@end SCDisplayFrame content construction
}

/*!
 * Should we show tooltips?
 */

bool SCDisplayFrame::ShowToolTips()
{
	return true;
}

/*!
 * Get bitmap resources
 */

wxBitmap SCDisplayFrame::GetBitmapResource( const wxString& name )
{
	// Bitmap retrieval
////@begin SCDisplayFrame bitmap retrieval
	wxUnusedVar(name);
	return wxNullBitmap;
////@end SCDisplayFrame bitmap retrieval
}

/*!
 * Get icon resources
 */

wxIcon SCDisplayFrame::GetIconResource( const wxString& name )
{
	// Icon retrieval
////@begin SCDisplayFrame icon retrieval
	wxUnusedVar(name);
	return wxNullIcon;
////@end SCDisplayFrame icon retrieval
}

/*!
 * Member initialisation 
 */

void SCDisplayFrame::Init()
{
////@begin SCDisplayFrame member initialisation
	engine = NULL;
////@end SCDisplayFrame member initialisation
}

void SCDisplayFrame::InitGL()
{
#ifndef __WXMOTIF__
    if (!this->GetContext()) return;
#endif
    this->SetCurrent();
    // Culling
    glFrontFace(GL_CCW);
    glCullFace(GL_BACK);//FRONT);
    glEnable(GL_CULL_FACE);
    // Depth test
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);
    // Cleaning
    glClearColor( BG_COLOUR, 1.0f );
    // Set the viewpoint
	glLoadIdentity();
	gluLookAt(
        0.f, 0.f, 1.f,
        0.f, 0.f, -1.f,
        0.f, 1.f, 0.f
    );
    // Quadratic objects
    quadratic = gluNewQuadric(); // Create a pointer to the quadric object
    //gluQuadricOrientation(quadratic,GLU_OUTSIDE);
	gluQuadricNormals(quadratic,GLU_SMOOTH); // Create smooth normals
    // Create the display lists
	sphere_list = glGenLists(1);
	glNewList(sphere_list,GL_COMPILE);
	{
        gluSphere(quadratic,1.f,SUBDIVISIONS,SUBDIVISIONS);
	}
	glEndList();
	cone_list = glGenLists(1);
	glNewList(cone_list,GL_COMPILE);
	{
        gluCylinder(quadratic,0.03f,0.f,CONE_HEIGHT,30,30);
	}
	glEndList();
	
}

/*!
 * wxEVT_SIZE event handler for ID_SCDISPLAYFRAME
 */

void SCDisplayFrame::OnSize( wxSizeEvent& event )
{
	wxGLCanvas::OnSize(event);
    // Set GL viewport (not called by wxGLCanvas::OnSize on all platforms...)
    int w, h;
    this->GetClientSize(&w, &h);
#ifndef __WXMOTIF__
    if ( this->GetContext() )
#endif
	{
		this->SetCurrent();
		glViewport(0, 0, (GLint) w, (GLint) h);
		// Set View
		glMatrixMode( GL_PROJECTION );
		glLoadIdentity();
		glOrtho(-1.f, 1.f, -1.f, 1.f, 0.f, 1.f);
		glMatrixMode( GL_MODELVIEW );
	}
////@begin wxEVT_SIZE event handler for ID_SCDISPLAYFRAME in SCDisplayFrame.
	// Before editing this code, remove the block markers.
	event.Skip();
////@end wxEVT_SIZE event handler for ID_SCDISPLAYFRAME in SCDisplayFrame. 
}

void SCDisplayFrame::buildGraphStructure(const vector<SCSystem> & systems) {
	this->net_graph.Clear();
	for (unsigned int i=0; i<systems.size(); ++i) {
		if (Type(systems[i]) == Axon) {
			if (systems[i].GetSchema2(0,16).find_first_of("1") != string::npos)
				this->net_graph.AddNode(Triple<string,const SCSystem*,Point3D<float> >("input",&systems[i],Point3D<float>()));
			/*
			for (unsigned int j=0; j<systems[i].NbSubSystems(); ++j)
				if (Type(systems[i].GetSubSystem(j)) == InputTransfer)
					this->net_graph.AddNode(Triple<string,const SCSystem*,Point3D<float> >("input",&systems[i],Point3D<float>()));
			*/
		}
		else if (Type(systems[i]) == Soma) {
			//bool output = false;
			const SCSystem & soma = systems[i];
			for (unsigned int s=0; s<soma.NbSubSystems(); ++s) {
				if ((Type(soma.GetSubSystem(s)) == Axon)) {
					/*
					for (unsigned int k=0; k<soma.GetSubSystem(j).NbSubSystems(); ++k)
						if (Type(soma.GetSubSystem(j).GetSubSystem(k)) == OutputTransfer)
							output = true;
			if (output)*/
					if (systems[i].GetSubSystem(s).GetSchema2(16,16).find_first_of("1") != string::npos)
						this->net_graph.AddNode(Triple<string,const SCSystem*,Point3D<float> >("output",&systems[i],Point3D<float>()));
					else this->net_graph.AddNode(Triple<string,const SCSystem*,Point3D<float> >("hidden",&systems[i],Point3D<float>()));
				}
			}
		}
	}
}

void SCDisplayFrame::computeNodesLocation() {
	// Canvas size
	int w, h;
    this->GetClientSize(&w, &h);
	// Calculate the number of nodes and lines
	unsigned int nb_in = 0, nb_hid = 0, nb_out = 0, nb_lines, nb_per_line;
	for (unsigned int i=0; i<this->net_graph.GetNbNodes(); ++i) {
		if (this->net_graph.GetNode(i).GetData().GetFirst() == "input")
			++nb_in;
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "hidden")
			++nb_hid;
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "output")
			++nb_out;
	}
	nb_per_line = (unsigned int)(2.f/(SYS_SIZE+SYS_SPACE));
	nb_lines = nb_hid / nb_per_line + ((nb_hid%nb_per_line)>0)?1:0;
	// First line on the left: Inputs
	// Middle lines: Hidden neurons
	// Last line on the right: Outputs
	unsigned int idx_in = 0, idx_hid = 0, idx_out = 0;
	for (unsigned int i=0; i<this->net_graph.GetNbNodes(); ++i) {
		if (this->net_graph.GetNode(i).GetData().GetFirst() == "input") {
			this->net_graph.Node(i).Data().Third() = Point3D<float>(
				-1.f + (SYS_SPACE+SYS_SIZE)/2.f,
				1.f - (((float)idx_in+0.5f)*2.f) / ((float)nb_in),
				0.f
			);
			++idx_in;
		}
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "hidden") {
			unsigned int	line = idx_hid/nb_per_line,
							nb_on_this_line = ((line+1)*nb_per_line < nb_hid)?nb_per_line:(nb_hid%nb_per_line);
			this->net_graph.Node(i).Data().Third() = Point3D<float>(
				-1.f + (3.f*SYS_SPACE)/2.f + SYS_SIZE + ((float)line+0.5f) * ((2.f-2.f*SYS_SIZE-3.f*SYS_SPACE)/((float)nb_lines)),
				1.f - ((((float)(idx_hid%nb_per_line))+0.5f)*2.f) / ((float)nb_on_this_line),
				0.f
			);
			++idx_hid;
		}
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "output") {
			this->net_graph.Node(i).Data().Third() = Point3D<float>(
				1.f - (SYS_SPACE+SYS_SIZE)/2.f,
				1.f - (((float)idx_out+0.5f)*2.f) / ((float)nb_out),
				0.f
			);
			++idx_out;
		}
	}
}

void SCDisplayFrame::ReloadData() {
	if (this->engine == NULL) return;
	this->buildGraphStructure(this->engine->GetSystems());
	this->computeNodesLocation();
}

const int SCDisplayFrame::findNodeIndexFromSystem(const SCSystem & sys) {
	for (int i=0; i<(int)this->net_graph.GetNbNodes(); ++i)
		if (this->net_graph.GetNode(i).GetData().GetSecond() == &sys)
			return i;
	return -1;
}

void SCDisplayFrame::setGraphArcs(const vector<SCSystem> & systems) {
	this->net_graph.ClearArcs();
	int index;
	for (unsigned int i=0; i<this->net_graph.GetNbNodes(); ++i) {
		if (Type(*this->net_graph.GetNode(i).GetData().GetSecond()) != Soma) continue;
		const SCSystem & soma = *this->net_graph.GetNode(i).GetData().GetSecond();
		for (unsigned int j=0; j<soma.NbSubSystems(); ++j) {
			if (Type(soma.GetSubSystem(j)) != Dendrites) continue;
			const SCSystem & dendrites = soma.GetSubSystem(j);
			for (unsigned int k=0; k<dendrites.NbSuperSystems(); ++k) {
				if (Type(dendrites.GetSuperSystem(k)) != SynapticConnection) continue;
				const SCSystem & connection = dendrites.GetSuperSystem(k);
				for (unsigned int l=0; l<connection.NbSubSystems(); ++l) {
					if (Type(connection.GetSubSystem(l)) != Axon) continue;
					const SCSystem & axon = connection.GetSubSystem(l);
/*
			for (unsigned int k=0; k<dendrites.NbSubSystems(); ++k) {
				if (Type(dendrites.GetSubSystem(k)) != Synapse) continue;
				const SCSystem & synapse = dendrites.GetSubSystem(k);
				for (unsigned int l=0; l<synapse.NbSuperSystems(); ++l) {
					if (Type(synapse.GetSuperSystem(l)) != Axon) continue;
					const SCSystem & axon = synapse.GetSuperSystem(l);
*/
					if ((index = this->findNodeIndexFromSystem(axon)) != -1)
						this->net_graph.Node(index).AddArc(Types::Arc<>(i));
					else {
						for (unsigned int m=0; m<axon.NbSuperSystems(); ++m)
							if ((Type(axon.GetSuperSystem(m)) == Soma) && ((index = this->findNodeIndexFromSystem(axon.GetSuperSystem(m))) != -1))
								this->net_graph.Node(index).AddArc(Types::Arc<>(i));
					}
				}
			}
		}
	}
}

void SCDisplayFrame::drawGraph() {
	/// --- Draw Nodes ---
	for (unsigned int i=0; i<this->net_graph.GetNbNodes(); ++i) {
		const Point3D<float> & pos = this->net_graph.GetNode(i).GetData().GetThird();
		// Save matrix
		glPushMatrix();
		// Set the location
		glTranslatef(pos.x, pos.y, pos.z);
		// Draw
		if (this->net_graph.GetNode(i).GetData().GetFirst() == "input")
			glColor3f(INPUT_COLOUR);
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "hidden")
			glColor3f(HIDDEN_COLOUR);
		else if (this->net_graph.GetNode(i).GetData().GetFirst() == "output")
			glColor3f(OUTPUT_COLOUR);
		else glColor3f(0.f,0.f,0.f); // Should not happen
		glScalef(SYS_SIZE/2.f,SYS_SIZE/2.f,SYS_SIZE/2.f);
		glCallList(sphere_list);
		// Restore matrix
		glPopMatrix();
		/// --- Draw Arcs ---
		for (unsigned int j=0; j<this->net_graph.GetNode(i).GetNbArcs(); ++j) {
			const Point3D<float> & pos2 = this->net_graph.GetNode(this->net_graph.GetNode(i).GetArc(j).GetTargetNodeIndex()).GetData().GetThird();
			// Save matrix
			glPushMatrix();
			glTranslatef(pos.x, pos.y, pos.z);
			glColor3f(ARC_COLOUR);
			if (pos != pos2) {		
				Vector3D<float> arc = pos2 - pos;
				//glTranslatef( (-arc.GetY()/arc.Norm()) * SYS_SIZE * 0.5f * frand(), (arc.GetX()/arc.Norm()) * SYS_SIZE * 0.5f * frand(), 0.f);
				float	height = arc.Norm(),
						angle = acos(sqrt(arc.x*arc.x+arc.y*arc.y)/height)*180./M_PI;
				if (arc.z > 0.f) angle = 90.f - angle;
				else angle = 90.f + angle;
				glRotatef(angle,-arc.y,arc.x,0.f);
				gluCylinder(quadratic,0.01f,0.01f,height-SYS_SIZE/2.f-CONE_HEIGHT,10,10);
				for (unsigned int k=0; k<NB_ARROWS; ++k) {
					glTranslatef(0.f,0.f,(height-SYS_SIZE/2.f)/NB_ARROWS-CONE_HEIGHT);
					glCallList(cone_list);
					glTranslatef(0.f,0.f,CONE_HEIGHT);
				}
			}
			else {
				Point3D<float> pos3 = pos + Vector3D<float>(SYS_SIZE+SYS_SPACE,SYS_SIZE/2.f,0.f);
				Vector3D<float> arc = pos3 - pos;
				float	height = arc.Norm(),
						angle = acos(sqrt(arc.x*arc.x+arc.y*arc.y)/height)*180./M_PI;
				if (arc.z > 0.f) angle = 90.f - angle;
				else angle = 90.f + angle;
				glRotatef(angle,-arc.y,arc.x,0.f);
				gluCylinder(quadratic,0.01f,0.01f,height-SYS_SIZE/2.f-CONE_HEIGHT,10,10);
			}
			// Restore matrix
			glPopMatrix();
		}
	}
}

void SCDisplayFrame::OnPaint( wxPaintEvent& event )
{
////@begin wxEVT_PAINT event handler for ID_SCDISPLAYFRAME in SCDisplayFrame.
	// Before editing this code, remove the block markers.
	wxPaintDC dc(this);
////@end wxEVT_PAINT event handler for ID_SCDISPLAYFRAME in SCDisplayFrame. 
	#ifndef __WXMOTIF__
    if (!GetContext()) return;
	#endif
	// Make the canvas the current recipient of OpenGL 
	this->SetCurrent();
	// Clear
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
	// Check whether there are systems
	if ((this->engine != NULL) && (this->engine->GetSystems().size() > 0)) {
		this->setGraphArcs(this->engine->GetSystems());
		this->drawGraph();
	}
	// Flush
	glFlush();
	// Swap
	this->SwapBuffers();
}

/*!
 * wxEVT_PAINT event handler for ID_SCDISPLAYFRAME
 */
/*
void SCDisplayFrame::OnPaint( wxPaintEvent& event )
{
////@begin wxEVT_PAINT event handler for ID_SCDISPLAYFRAME in SCDisplayFrame.
	// Before editing this code, remove the block markers.
	wxPaintDC dc(this);
////@end wxEVT_PAINT event handler for ID_SCDISPLAYFRAME in SCDisplayFrame. 
	#ifndef __WXMOTIF__
    if (!GetContext()) return;
	#endif
	// Make the canvas the current recipient of OpenGL 
	this->SetCurrent();
	// Clear
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
	// Check whether there are systems
	if ((this->engine != NULL) && (this->engine->GetSystems().size() > 0)) {
		// Render
		for (unsigned int i=0; i<this->engine->GetSystems().size(); ++i) {
			const SCSystem & sys = this->engine->GetSystems()[i];
			// Save matrix
			glPushMatrix();
			// Set the location
			glTranslatef(
				this->sys_locations[&sys].GetX(),
				this->sys_locations[&sys].GetY(),
				this->sys_locations[&sys].GetZ()
			);
			// Set scale
			glScalef(this->sys_sizes[&sys],this->sys_sizes[&sys],this->sys_sizes[&sys]);
			// Draw the outline
			glColor3f(0.f,0.f,0.f);
			glCallList(sphere_list);
			// Draw inside
			glScalef(0.99f,0.99f,0.99f);
			colour & col = this->colours[this->types[i]];
			glColor3f(col.r,col.g,col.b);
			glCallList(sphere_list);
			// Restore matrix
			glPopMatrix();
		}
		// Show the last computation
		Vector3D<float> link;
		float height, angle;
		for (int i=0; i<((int)this->last_involved.size())-1; ++i) {
			glPushMatrix();
			link = this->sys_locations[this->last_involved[i+1]] - this->sys_locations[this->last_involved[i]];
			height = link.Norm(),
			angle = acos(sqrt(link.GetX()*link.GetX()+link.GetY()*link.GetY())/height)*180./M_PI;
            if (link.GetZ() > 0.f) angle = 90.f - angle;
            else angle = 90.f + angle;
            glTranslatef(
                this->sys_locations[this->last_involved[i]].GetX(),
                this->sys_locations[this->last_involved[i]].GetY(),
                this->sys_locations[this->last_involved[i]].GetZ()
            );
            glRotatef(angle,-link.GetY(),link.GetX(),0.f);
            glColor3f(1.f,1.f,1.f);
			gluCylinder(quadratic,0.01f,0.01f,height,30,30);
			glPopMatrix();
		}
	}
	// Flush
	glFlush();
	// Swap
	this->SwapBuffers();
}

void SCDisplayFrame::ReloadData(const vector<unsigned int> & types) {
	if (this->engine == NULL) return;
	// Set types
	this->types = types;
	// Reset data
	this->sys_sizes.clear();
	this->sys_locations.clear();
	// If there is no system, return
	if (this->engine->GetSystems().size() == 0) return;
	// Get the universe
	const SCSystem & universe = this->engine->GetUniverse();
	// Set sizes
	for (unsigned int i=0; i<this->engine->GetSystems().size(); ++i)
		this->sys_sizes[&(this->engine->GetSystems()[i])] = -1.f;
	vector<const SCSystem *> visited;
	this->setSizes(universe,visited);
	// Rescale
	float u_size = this->sys_sizes[&universe];
	for (unsigned int i=0; i<this->engine->GetSystems().size(); ++i)
		this->sys_sizes[&(this->engine->GetSystems()[i])] /= u_size;
	// Place systems
	this->PlaceSystems();
}

const float SCDisplayFrame::setSizes(
	const SCSystem & sys,
	vector<const SCSystem *> & visited
) {
	// Assert there is no recursive hierarchy
	if (find(visited.begin(),visited.end(),&sys) != visited.end())
		throw AssertionException("A neural network should not have recursive hierarchy");
	// Compute size
	float sys_size = 0.f;
	// If leaf, size of 1.f
	if (sys.GetSubSystems().size() == 0) sys_size = 1.f;
	// If not, compute the size of the sub-systems and sum them up to define the current system's size
	else {
		visited.push_back(&sys);
		for (unsigned int s=0; s<sys.GetSubSystems().size(); ++s)
			sys_size += setSizes(sys.GetSubSystem(s),visited);
		visited.pop_back();
		sys_size *= 0.5f + 1.0f/((float)sys.GetSubSystems().size());
		// Check now that the computed size is compatible with eventual precomputing of the current system's size
		if ((this->sys_sizes[&sys] != -1.f) && (this->sys_sizes[&sys] !=sys_size)) {
			stringstream ss;
			ss << "System's previously computed size " << this->sys_sizes[&sys] << " does not match the newly computed size " << sys_size;
			throw SizeException(ss.str());
		}
	}
	// Set size
	this->sys_sizes[&sys] = sys_size;
	// Return size
	return sys_size;
}

// Maximum number of attempts to satisfy the constraints
#define MAX_ATTEMPTS	4
// Maximum number of iterations to satisfy the constraints
#define MAX_ITERATIONS	20000
void SCDisplayFrame::PlaceSystems() {
	// Get the universe
	const SCSystem & universe = this->engine->GetUniverse();
	unsigned int nb_attempts = 0;
	bool done;
	do {
		// Randomly assign a position to all of the systems (within the universe)
		for (unsigned int i=0; i<this->engine->GetSystems().size(); ++i) {
			const SCSystem & sys = this->engine->GetSystems()[i];
			float angle = (((float)rand())/((float)RAND_MAX))*2.f*M_PI;
			float dist = (((float)rand())/((float)RAND_MAX))*this->sys_sizes[&universe];
			this->sys_locations[&sys].X() = dist * cos(angle);
			this->sys_locations[&sys].Y() = dist * sin(angle);
			this->sys_locations[&sys].Z() = 0.f;
		}
		// Place universe to the origin
		this->sys_locations[&universe] = Point3D<float><float>(0.f,0.f,0.f);
		// Solve constraints
		done = this->satisfyConstraints();
		// One more attempt
		++nb_attempts;
	}
	while ((!done) && (nb_attempts < MAX_ATTEMPTS));
	// Check attemptsnumber
	if (!done)
		throw ConstraintSatisfactionException("Could not place systems correctly");
}

const bool SCDisplayFrame::satisfyConstraints() {
	// As long as a system with unsatisfied constraints can be found
	// Or if MAX_ITERATIONS is reached
	Constraint constraint;
	const SCSystem & universe = this->engine->GetUniverse();
	unsigned int nb_iter = 0;
	while (this->pickupConstraint(constraint) && (nb_iter < MAX_ITERATIONS)) {
		// Check zero division
		if (constraint.dist == 0.f) {
			constraint.dist = 0.1f;
			cerr << "Division By Zero attempted : Constraint distance set to " << constraint.dist << endl;
		}
		// Compute distance error
		float error = constraint.req_dist - constraint.dist;
		// Compute vector
		Vector3D<float> sys1sys2 = this->sys_locations[constraint.sys2] - this->sys_locations[constraint.sys1];
		// Compute displacement (for each system between 0 and error)
		float disp;
		if (constraint.sys1 == &universe) {
			disp = (((float)rand())/((float)RAND_MAX))*2.f*error;
			this->sys_locations[constraint.sys2] += sys1sys2*(disp/constraint.dist);
		}
		else if (constraint.sys2 == &universe) {
			disp = (((float)rand())/((float)RAND_MAX))*2.f*error;
			this->sys_locations[constraint.sys1] -= sys1sys2*(disp/constraint.dist);
		}
		else {
			disp = (((float)rand())/((float)RAND_MAX))*error;
			this->sys_locations[constraint.sys1] -= sys1sys2*(disp/constraint.dist);
			disp = (((float)rand())/((float)RAND_MAX))*error;
			this->sys_locations[constraint.sys2] += sys1sys2*(disp/constraint.dist);
		}
		// Increment the number of iterations
		++nb_iter;
	}
	// Check iteration number
	if (nb_iter >= MAX_ITERATIONS)
		return false;
	return true;
}

const bool SCDisplayFrame::pickupConstraint(Constraint & constraint) {
	// Create the list of all constraints
	vector<Constraint> constraints;
	for (unsigned int i=0; i<this->engine->GetSystems().size(); ++i) {
		const SCSystem & sys = this->engine->GetSystems()[i];
		vector<Constraint> sys_const = this->getSystemUpwardConstraints(sys);
		for (unsigned int j=0; j<sys_const.size(); ++j) {
			if (find(constraints.begin(),constraints.end(),sys_const[j]) == constraints.end())
				constraints.push_back(sys_const[j]);
		}
	}
	// If there is no constraint then the system is stable
	if (constraints.size() == 0)
		return false;
	// Otherwise, choose a constraint
	else {
		constraint = constraints[rand()%constraints.size()];
		return true;
	}
}

// Max & Min
#define MAX(X,Y) ((X)>(Y)?(X):(Y))
#define MIN(X,Y) ((X)<(Y)?(X):(Y))
// Float comparison epsilon value (zero approximation)
#define FLOAT_EPSLION 0.0001
// Overlapping quantity
#define SYS_OVERLAP 0.75f
vector<Constraint> SCDisplayFrame::getSystemUpwardConstraints(const SCSystem & sys) {
	vector<Constraint> constraints;
	Constraint c;
	c.sys1 = &sys;
	cout.precision(10);
	// Check super and sibling systems constraints
	for (unsigned int i=0; i<sys.GetSuperSystems().size(); ++i) {
		const SCSystem & parent = sys.GetSuperSystem(i);
		// Check that the system is (completely or partially) inside the parent
		// Partially inside only occurs when a system has another parent, itself
		// parent of this parent. In such a case, to belong to both, the system
		// must be shared between inside one parent and outside of this same one
		// making it inside the other parent.
		bool partially = false;
		for (unsigned int j=0; j<sys.GetSuperSystems().size() && !partially; ++j)
			if ((&parent != &sys.GetSuperSystem(j)) && (parent.HasSuperSystem(sys.GetSuperSystem(j))))
				partially = true;
		c.dist = (float)((this->sys_locations[&sys] - this->sys_locations[&parent]).Norm());
		// Partially inside case
		if (partially) {
			// Must not be completely outside
			c.req_dist = (float)fabs(this->sys_sizes[&parent]+this->sys_sizes[&sys]-SYS_OVERLAP*MIN(this->sys_sizes[&parent],this->sys_sizes[&sys]));
			if (c.dist > c.req_dist + FLOAT_EPSLION) {
				//cout << "ADD1 " << c.dist << " " << c.req_dist << endl;
				c.sys2 = &parent;
				constraints.push_back(c);
			}
			// Must not be completely inside
			c.req_dist = (float)fabs(this->sys_sizes[&parent]-this->sys_sizes[&sys]+SYS_OVERLAP*MIN(this->sys_sizes[&parent],this->sys_sizes[&sys]));
			if (c.dist + FLOAT_EPSLION < c.req_dist) {
				//cout << "ADD2 " << c.dist << " " << c.req_dist << endl;
				c.sys2 = &parent;
				constraints.push_back(c);
			}
		}
		// Fully inside case
		else {
			c.req_dist = (float)fabs(this->sys_sizes[&parent]-this->sys_sizes[&sys]);
			if (c.dist > c.req_dist + FLOAT_EPSLION) {
				//cout << "ADD3 " << c.dist << " " << c.req_dist << endl;
				c.sys2 = &parent;
				constraints.push_back(c);
			}
		}
		// Check siblings collision
		for (unsigned int j=0; j<parent.GetSubSystems().size(); ++j) {
			const SCSystem & sibling = parent.GetSubSystem(j);
			// Check that the parent's child is not the current system
			if (&sys != &sibling) {
				c.dist = (float)((this->sys_locations[&sys] - this->sys_locations[&sibling]).Norm());
				// If the system shares something with the sibling then one must not include the other
				if (sys.DoesShareWith(sibling)) {
					c.req_dist = (float)fabs(this->sys_sizes[&sibling]-this->sys_sizes[&sys]);
					if (c.dist + FLOAT_EPSLION < c.req_dist) {
						//cout << "ADD4 " << c.dist << " " << c.req_dist << endl;
						c.sys2 = &sibling;
						constraints.push_back(c);
					}
				}
				// If the system is not related to that sibling in a parent/child
				// relation (partial parentship) then they cannot overlap
				else if (!sys.HasSubSystem(sibling) && !sys.HasSuperSystem(sibling)) {
					c.req_dist = (float)fabs(this->sys_sizes[&sibling]+this->sys_sizes[&sys]);
					if (c.dist + FLOAT_EPSLION < c.req_dist) {
						//cout << "ADD5 " << c.dist << " " << c.req_dist << endl;
						c.sys2 = &sibling;
						constraints.push_back(c);
					}
				}
				// Check parents of siblings not parent of the current system
				for (unsigned int k=0; k<sibling.GetSuperSystems().size(); ++k) {
					const SCSystem & step_parent = sibling.GetSuperSystem(k);
					// Check that step parent is not the current system (partial parentship)
					if (&sys != &step_parent) {
						// If it's not a parent or a grand-parent (partial parentship)
						if (!sys.HasSuperSystem(step_parent) && !parent.HasSuperSystem(step_parent) && (!sys.DoesShareWith(step_parent))) {
							// The current system must be outside
							c.dist = (float)((this->sys_locations[&sys] - this->sys_locations[&step_parent]).Norm());
							c.req_dist = (float)fabs(this->sys_sizes[&step_parent]+this->sys_sizes[&sys]);
							if (c.dist + FLOAT_EPSLION < c.req_dist) {
								//cout << "ADD6 " << c.dist << " " << c.req_dist << endl;
								c.sys2 = &step_parent;
								constraints.push_back(c);
							}
						}
					}
				}
			}
		}
	}
	//cout << "--------------" << endl;
	return constraints;
}
*/

